#include "KF.h"
#include "IMU.h"
#include "hal_data.h"

//定义加速度计的量程,单位为g
#define ACC_RANGE 8.0f
//定义陀螺仪的量程,单位为度每秒
#define GYRO_RANGE 1000.0f
//定义偏置取平均的次数
#define imu_bias_num 1

uint8_t Read_Register(uint8_t reg);

void Write_Register(uint8_t reg, uint8_t value);

void IMU_Output(float *result);

void IMU_Get_Value(float *value);

//定义三轴陀螺仪的偏置值
float gyro_offset[3] = {0};

#define CS_PIN BSP_IO_PORT_04_PIN_01

volatile bool g_transfer_complete = false;
void sci_b_spi_callback (spi_callback_args_t * p_args)
{
    if (SPI_EVENT_TRANSFER_COMPLETE == p_args->event)
    {
        g_transfer_complete = true;
    }
}


//读寄存器值
//输入参数为寄存器地址
//输出参数为寄存器值
uint8_t Read_Register(uint8_t reg)
{
    uint8_t value = 0;
    uint8_t address = 0;
    //作为读，需要将地址的最高位设置为1
    address = reg | 0x80;

    //进行片选
    R_BSP_PinWrite(CS_PIN, BSP_IO_LEVEL_LOW);

    //读取寄存器值的具体实现
    R_SCI_B_SPI_Write(&g_sci_spi0_ctrl, &address, 1, SPI_BIT_WIDTH_8_BITS);
    while (false == g_transfer_complete);
    g_transfer_complete = false;

    R_SCI_B_SPI_Read(&g_sci_spi0_ctrl, &value, 1, SPI_BIT_WIDTH_8_BITS);
    while (false == g_transfer_complete);
    g_transfer_complete = false;

    //结束片选
    R_BSP_PinWrite(CS_PIN, BSP_IO_LEVEL_HIGH);

    return value;
}

//写寄存器值
//输入参数为寄存器地址和寄存器值
//输出参数为空
void Write_Register(uint8_t reg, uint8_t value)
{
    uint8_t address = 0;
    //作为写，需要将地址的最高位设置为0
    address = reg & ~0x80;
    //进行片选
    R_BSP_PinWrite(CS_PIN, BSP_IO_LEVEL_LOW);

    //读取寄存器值的具体实现
    R_SCI_B_SPI_Write(&g_sci_spi0_ctrl, &address, 1, SPI_BIT_WIDTH_8_BITS);
    while (false == g_transfer_complete);
    g_transfer_complete = false;

    R_SCI_B_SPI_Write(&g_sci_spi0_ctrl, &value, 1, SPI_BIT_WIDTH_8_BITS);
    while (false == g_transfer_complete);
    g_transfer_complete = false;

    //结束片选
    R_BSP_PinWrite(CS_PIN, BSP_IO_LEVEL_HIGH);
}

//检测启动是否成功
uint8_t IMU_Restart_Ready(void)
{
    uint8_t reg[12];
    int16_t data[6]= {0};
    //读取IMU的加速度计和陀螺仪的值
    uint8_t reg_address = IMU_ACCEL_DATA_X1_UI;
    //读取12个寄存器的值
    for (uint8_t i = 0; i < 12; i++)
    {
        reg[i] = Read_Register(reg_address);
        reg_address++;
    }

    //将reg的值分高低位存储在data中
    for (int i = 0; i < 6; i++)
    {
        data[i] = (int16_t)((reg[2 * i + 1] << 8) | reg[2 * i]);
    }

    //崩溃检测和重启
    if(data[0] == 0 && data[1] == 0 && data[2] == 0)
    {
        //重启失败
        return 0;
    }
    return 1;
    //重启成功
}

//imu重启
void IMU_Restart(void)
{
    do
    {

        //执行IMU初始化
        uint8_t regvalue = 0x00;
        //执行IMU的复位
        regvalue = 0x02;
        Write_Register(IMU_REG_MISC2, regvalue);
        R_BSP_SoftwareDelay(100, BSP_DELAY_UNITS_MILLISECONDS);

        //    //设置IMU的工作模式为SPI模式
        //    regvalue = 0x61;
        //    Write_Register(IMU_DRIVE_CONFIG0, regvalue);
        //    R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MILLISECONDS);

        //设置加速度计
        /*ACC_CONFIG寄存器配置如下
        0-3设置加速度计的采样频率
        设置为0111，对应为400Hz
        4-6设置加速度计的量程
        设置为010，对应为±8g*/
        regvalue = 0x27;
        Write_Register(IMU_ACCEL_CONFIG0, regvalue);
        R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MILLISECONDS);

        //设置陀螺仪
        /*GYRO_CONFIG寄存器配置如下
        0-3设置陀螺仪的采样频率
        设置为0111，对应为400Hz
        4-7设置陀螺仪的量程
        设定为0010，对应为±1000度每秒
        */
        regvalue = 0x27;
        Write_Register(IMU_GYRO_CONFIG0, regvalue);
        R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MILLISECONDS);


       //使能加速度计和陀螺仪
       //执行pwr的复位
        /*PWR寄存器配置如下
        0-1配置加速度计的状态：
            00关闭
            01关闭
            10开启低功耗模式
            11开启低噪声模式
        2-3配置陀螺仪的状态
            00关闭
            01陀螺仪处于待机模式
            10开启低功耗模式
            11开启低噪声模式*/
        regvalue = 0x0F;
        Write_Register(IMU_PWR_MGMT0, regvalue);
        R_BSP_SoftwareDelay(100, BSP_DELAY_UNITS_MILLISECONDS);

    }while(IMU_Restart_Ready() == 0);
}

//执行传感器初始化
void IMU_Init(void)
{

    R_BSP_PinAccessEnable();
    //启动SPI
    R_SCI_B_SPI_Open(&g_sci_spi0_ctrl, &g_sci_spi0_cfg);

    //imu启动
    IMU_Restart();

    //执行偏置值的初始化
    //读取一次数据
    float value[6] = {0};
    for(uint8_t i = 0; i < 3; i++)
    {
        gyro_offset[i] = 0.0f;
    }
    //去平均
    for(uint32_t j = 0; j < imu_bias_num; j ++)
    {
        IMU_Get_Value(value);
        //将陀螺仪偏置值存储在全局变量中
        for (int i = 0; i < 3; i++)
        {
            gyro_offset[i] += value[i + 3];
        }
        R_BSP_SoftwareDelay(5, BSP_DELAY_UNITS_MILLISECONDS);
    }
    //初始化
    for(uint8_t i = 0; i < 3; i++)
    {
        gyro_offset[i] /= (float)imu_bias_num;
    }
    //计算初始静态下的矢量合加速度
    float acc_init = 0;
    acc_init = sqrtf(value[0] * value[0] + value[1] * value[1] + value[2] * value[2]);

   //初始化卡尔曼滤波器
   KF_Init(&acc_init);

   R_BSP_SoftwareDelay(30, BSP_DELAY_UNITS_MILLISECONDS);
   //启动IMU的定时器
   R_GPT_Open(&g_timer0_ctrl, &g_timer0_cfg);
   R_GPT_Start(&g_timer0_ctrl);
}

//获取IMU的加速度计和陀螺仪的值，需要读取12个寄存器
//输入参数为float*类型的指针，长度为6
//输出参数为空
void IMU_Get_Value(float *value)
{

//    uint8_t temp = 0;
//    temp = Read_Register(0x72);
//    R_SCI_B_UART_Write(&g_uart0_ctrl, &temp, 2);

    uint8_t reg[12];
    int16_t data[6]= {0};
    //读取IMU的加速度计和陀螺仪的值
    uint8_t reg_address = IMU_ACCEL_DATA_X1_UI;
    //读取12个寄存器的值
    for (uint8_t i = 0; i < 12; i++)
    {
        reg[i] = Read_Register(reg_address);
        reg_address++;
    }
    
    //将reg的值分高低位存储在data中
    for (int i = 0; i < 6; i++)
    {
        data[i] = (int16_t)((reg[2 * i + 1] << 8) | reg[2 * i]);
    }

    //崩溃检测和重启
    if(data[0] == 0 && data[1] == 0 && data[2] == 0)
    {
        //发现崩溃，执行重启
        IMU_Restart();
    }

    //将加速度的值转换为float类型
    for (int i = 0; i < 3; i++)
    {
        value[i] = (float)data[i] / (32768.0f / ACC_RANGE );
    }

    //将陀螺仪的值转换为float类型
    for (int i = 3; i < 6; i++)
    {
        value[i] = (float)data[i] / (32768.0f / GYRO_RANGE);
        //将陀螺仪的值进行偏置值的补偿
//        value[i] -= gyro_offset[i - 3];
    }
}

//执行结果的输出
void IMU_Output(float *result)
{
    //输出结果的具体实现
    //----------
}

//执行单次数据的更新
//输入为float数组，输出为空
void IMU_Update(float* result)
{
    float value[6] = {0};
    //获取IMU的加速度计和陀螺仪的值
    IMU_Get_Value(value);
    //执行卡尔曼滤波算法
    KF_Update(value, result);
//    //输出结果
//    IMU_Output(result);
}
